// -----------------------------------------------------------------------------
// M A R I T I M E   R E S E A R C H   I N S T I T U T E   N E T H E R L A N D S
// -----------------------------------------------------------------------------
// Program  : mermaid
// Module   : interactive waves
// File     : PulseControl.cpp
// -----------------------------------------------------------------------------
// Revisions:
// December 12, 2008  asicherer
// -----------------------------------------------------------------------------

#include <algorithm>
#include "defines.h"
#ifndef _WINDOWS
#include "mermaid.h"
#endif
#include "FileReader.h"
#include "FloatUtils.h"
#include "Interpolator.h"
#include "PulseControl.h"
#include "PulseManager.h"
#include "GeometryManager.h"
#include "MathGeometry.h"
#include "ShipGeometry.h"
#include "assertplus.h"

#define BEFORE -1                      // flag for position before path
#define AFTER  -2                      // flag for position after path

/// computes distance between two points
static REAL computeDistance(REAL x1, REAL y1, REAL x2, REAL y2)
{
    return sqrt(FloatUtils::square(x2 -x1) + FloatUtils::square(y2 - y1));
}    

/// computes length of vector
//static REAL computeLength(REAL v1, REAL v2)
//{
//    return sqrt(FloatUtils::square(v1) + FloatUtils::square(v2));
//}


/// computes angle between vectors
static REAL computeAngle(REAL v1, REAL v2)
{
    REAL v  = sqrt(v1 * v1 + v2 * v2);
    if (fabs(v) < 0.000001)
    {
        return 0;
    }
    REAL angle = asin(v1/v);
    if (v2 < 0)
    {
        angle = PI - angle;
    }
    if (angle < 0)
    {
        angle += 2 * PI;
    }
    return angle;
}

/// constructor
PulseControl::PulseControl(int steps,     ///< number of simulation steps
                           REAL timestep,///< length of timesteps in seconds
                           int Nx1,       ///< size in points in x1 direction
                           int Nx2,       ///< size in points in x2 direction
                           REAL dx1,     ///< grid step size in x1 direction
                           REAL dx2,     ///< grid step size in x2 direction
                           REAL pstart)
    : m_smoothing(false)
    , m_steps(steps)
    , m_timestep(timestep)
    , m_previoustime(0)
    , m_currentPoint()
    , m_path()
    , m_pulseData()
    , m_pPulseManager(0)
    , m_pGeometryManager(0)
    , m_moving(true)
{
    m_pPulseManager = new PulseManager(Nx1, Nx2, dx1, dx2, pstart, m_smoothing);
    m_pGeometryManager = new GeometryManager();
}
   
/// destructor
PulseControl::~PulseControl()
{
    delete m_pPulseManager;
    delete m_pGeometryManager;
}

/// reads pulse data from file
bool PulseControl::setupPulses(FileReader &fr)
{
    try 
    {
        m_smoothing = fr.ReadBoolean();
        m_pPulseManager->setSmoothing(m_smoothing);
        int n = fr.ReadInt();
        ASSERT(n < NRP, "n=" << n);
        for (int i = 0; i < n; ++i)
        {
            REAL r1    = fr.ReadFloat();
            REAL r2    = fr.ReadFloat();
            REAL x1    = fr.ReadFloat();
            REAL x2    = fr.ReadFloat();
            REAL v1    = fr.ReadFloat();
            REAL v2    = fr.ReadFloat();
            REAL a1    = fr.ReadFloat();
            REAL a2    = fr.ReadFloat();
            REAL pm    = fr.ReadFloat();
            std::string pulseFile = fr.ReadString();
            if (pulseFile == "-" || pulseFile == "+")
            {
                MathGeometry* pMath = new MathGeometry(mml::Vector3d(r1, r2, 0), pm, pulseFile== "-");
                pMath->setPosition(mml::Vector3d(x1, x2, 0));
                pMath->setAngle(computeAngle(v1, v2));
                m_pGeometryManager->add(pMath);
                m_pPulseManager->addPulse(pMath);
            }
            else
            {
                ShipGeometry* pShip = new ShipGeometry();
                if (!pShip->read(pulseFile))
                {
                    return false;
                }
                pShip->setPosition(mml::Vector3d(x1, x2, 0));
                m_pGeometryManager->add(pShip);
                m_pPulseManager->addPulse(pShip);
            }
            mml::Vector3d v(v1, v2, 0);
            mml::Vector3d a(a1, a2, 0);
            int signv = v1 + v2 >= 0 ? 1 : -1;
            int signa = a1 + a2 >= 0 ? 1 : -1;
            PulseData data(v, signv * v.norm(),
                           a, signa * a.norm());
            m_pulseData.push_back(data);
            m_currentPoint[i] = BEFORE;
            //m_pPulseManager->lower(i, 0.1);
        }
        return true;
    } 
    catch (...)
    {
        return false;
    }
}

/// reads pulse path data from file
bool PulseControl::setupPaths(FileReader &fr)
{
    try 
    {
        int n = m_pPulseManager->getNumberOfPulses();
        for (int i = 0; i < n; ++i)
        {
            m_waypoints[i].clear();
            int nr = fr.ReadInt();
            for (int j = 0; j < nr; ++j)
            {
                REAL x, y;
                fr.ReadFloats(&x, &y);
                m_waypoints[i].push_back(Point(x, y));
            }
            computeFirstWaypoint(i);
            computePath(i);
        }
        return true;
    } 
    catch (...)
    {
        return false;
    }
}

/// returns number of defined pulses
unsigned int PulseControl::getNumberOfPulses() const
{
    return m_pPulseManager->getNumberOfPulses();
}

/// determines vector that shows where pulse will enter field
/** returns false if pulse will never reach field */
bool PulseControl::getStartVector(int ship, Point *pstart, Point *pend)
{
    ASSERTPOINTER(pstart);
    ASSERTPOINTER(pend);
    if (!m_waypoints[ship].size())
    {
        computeFirstWaypoint(ship);
        if (!m_waypoints[ship].size())
        {
            return false;
        }
    }
    pstart->x1 = m_waypoints[ship][0].x1;
    pstart->x2 = m_waypoints[ship][0].x2;
    const mml::Vector3d& V = m_pulseData[ship].m_speed;
    pend->x1   = m_waypoints[ship][0].x1 - 4 * V(0);
    pend->x2   = m_waypoints[ship][0].x2 - 4 * V(1);
    return true;
}

/// determines first waypoint that is reached in field
void PulseControl::computeFirstWaypoint(int ship)
{
    int nx1;
    int nx2;
    REAL dx1;
    REAL dx2;
    m_pPulseManager->getGridProperties(&nx1, &nx2, &dx1, &dx2);
    REAL xx = nx1 * dx1;
    REAL yy = nx2 * dx2;
    REAL wpx = -1;
    REAL wpy = -1;
    int max = m_steps ? m_steps : 10000;
    for (int s = 0; s < max; ++s)
    {
        REAL t = s * m_timestep;
        const mml::Vector3d& X = m_pGeometryManager->getPosition(ship);
        const mml::Vector3d& V = m_pulseData[ship].m_speed;
        const mml::Vector3d& A = m_pulseData[ship].m_acceleration;
        REAL x = X(0) + V(0) * t + A(0) * FloatUtils::square(t);
        REAL y = X(1) + V(1) * t + A(1) * FloatUtils::square(t);
        if (x > 0 && x < xx && y > 0 && y < yy)
        {
            wpx = x;
            wpy = y;
            m_waypoints[ship].push_front(Point(wpx, wpy));
            break;
        }
    }
    ASSERT(m_waypoints[ship][0].x1 > -0.5, ship << " " << m_waypoints[ship][0].x1);
}

/// computes path to follow by interpolating waypoints
void PulseControl::computePath(int ship)
{
    unsigned int n = m_waypoints[ship].size();
    m_path[ship].clear();
    if (n < 2)
    {
        return;
    }
    Array1D<double> x(n);
    Array1D<double> y(n);
    for (unsigned int i = 0; i < n; ++i)
    {
        x[i] = m_waypoints[ship][i].x1;
        y[i] = m_waypoints[ship][i].x2;
    }
    Interpolator ip(x, y);
    Array1D<double> xx, yy;
    int nx1;
    int nx2;
    REAL dx1;
    REAL dx2;
    m_pPulseManager->getGridProperties(&nx1, &nx2, &dx1, &dx2);
    int nn = std::max(nx1, nx2);
    ip.interpolate(nn, &xx, &yy);
    for (int i = 0; i < nn; ++i)
    {
        m_path[ship].push_back(Point(xx[i], yy[i]));
    }
    
    // reverse path if starting point is nearer to end of path
    if (false /*nn*/)
    {
        const mml::Vector3d& X = m_pGeometryManager->getPosition(ship);
        REAL distanceStart = computeDistance(m_path[ship][0    ].x1, m_path[ship][0    ].x2, X(0), X(1));
        REAL distanceEnd   = computeDistance(m_path[ship][n - 1].x1, m_path[ship][n - 1].x2, X(0), X(1));
        if (distanceEnd < distanceStart)
        {
            std::reverse(m_path[ship].begin(), m_path[ship].end());
        }
    }
}


/// returns list of waypoints for specified pulse
const PulseControl::Waypoints &PulseControl::getWaypoints(int ship)
{
    return m_waypoints[ship];
}
    

/// returns list of path points for specified pulse
const PulseControl::Waypoints &PulseControl::getPath(int ship)
{
    return m_path[ship];
}


/// acc/decelerates all pulses to target speed/0
void PulseControl::toggleMovement()
{
    unsigned int n = m_pPulseManager->getNumberOfPulses();
    if (m_moving)
    {
        m_moving = false;
        for (unsigned int i = 0; i < n; ++i)
        {
            m_pulseData[i].m_currentScalarAcceleration = -m_pulseData[i].m_scalarAcceleration;
        }
    }
    else
    {
        m_moving = true;
        for (unsigned int i = 0; i < n; ++i)
        {
            m_pulseData[i].m_currentScalarAcceleration = m_pulseData[i].m_scalarAcceleration;
        }
    }
}

void PulseControl::startLowering(REAL currenttime)
{
    unsigned int n = m_pPulseManager->getNumberOfPulses();
    for (unsigned int i = 0; i < n; ++i)
    {
        m_pPulseManager->lower(i, currenttime);
    }
}

/// moves all pulses to new position and attitude
void PulseControl::movePulses(REAL currenttime)
{
    REAL delta = currenttime - m_previoustime;
    
    // compute current position of pulses
    unsigned int n = m_pPulseManager->getNumberOfPulses();
    for (unsigned int i = 0; i < n; ++i)
    {
        unsigned int n = m_path[i].size();
        bool definedPath = n > 0;
//        REAL distance = m_pulseData[i].m_currentScalarSpeed * delta +
  //                       0.5 * m_pulseData[i].m_currentScalarAcceleration * FloatUtils::square(delta);
        
        m_pulseData[i].m_currentScalarSpeed += m_pulseData[i].m_currentScalarAcceleration * delta;
        if (m_pulseData[i].m_currentScalarSpeed >= m_pulseData[i].m_scalarSpeed)
        {
            m_pulseData[i].m_currentScalarSpeed = m_pulseData[i].m_scalarSpeed;
//            printf("target speed ");
        }
        
        // if decelerating, stop at speed 0
        if (m_pulseData[i].m_currentScalarSpeed < 0.01 && m_pulseData[i].m_currentScalarAcceleration < 0)
        {
            m_pulseData[i].m_currentScalarSpeed = 0.0;
            m_pulseData[i].m_currentScalarAcceleration = 0.0;
//            printf("stopped ");
        }
//        printf("%.1f", m_pulseData[i].m_currentScalarSpeed);
        
        REAL distance = m_pulseData[i].m_currentScalarSpeed * delta;
//        std::cout << m_pulseData[i].m_currentScalarSpeed << '\n';
        REAL currentAngle = m_pGeometryManager->getAngle(i);
        const mml::Vector3d& X = m_pGeometryManager->getPosition(i);
        REAL x = X(0) + distance * sin(currentAngle);
        REAL y = X(1) + distance * cos(currentAngle);
        int nx1;
        int nx2;
        REAL dx1;
        REAL dx2;
        m_pPulseManager->getGridProperties(&nx1, &nx2, &dx1, &dx2);
        bool inside = (x >= 0 && x <= nx1 * dx1 && y >= 0 && y <= nx2 * dx2);
        if (definedPath && m_currentPoint[i] == BEFORE) // see if pulse just entered area
        {
            if (inside)
            {
                m_currentPoint[i] = 0;
            }
            else // still outside area
            {
                m_pGeometryManager->setAngle(i, computeAngle(m_path[i][0].x1 - X(0), m_path[i][0].x2 - X(1)));
                currentAngle = m_pGeometryManager->getAngle(i);
            }
        }
        if (definedPath && m_currentPoint[i] != AFTER && m_currentPoint[i] != BEFORE)
        {
            REAL distanceToNextPoint = 0;
            int p = m_currentPoint[i];
            REAL x = X(0);
            REAL y = X(1);
            for (;;)
            {
                if (p >= (int)n)
                {
                    m_currentPoint[i] = AFTER;
                    break;
                }
                distanceToNextPoint += computeDistance(m_path[i][p].x1, m_path[i][p].x2, x, y);
                if (distanceToNextPoint > distance)
                {
                    m_currentPoint[i] = p;
                    m_pGeometryManager->setAngle(i, computeAngle(m_path[i][p].x1 - X(0), m_path[i][p].x2 - X(1)));
                    currentAngle = m_pGeometryManager->getAngle(i);
                    distanceToNextPoint = computeDistance(m_path[i][p].x1, m_path[i][p].x2, X(0), X(1));
                    break;
                }
                x = m_path[i][p].x1;
                y = m_path[i][p].x2;
                ++p;
            }
        }
        m_pGeometryManager->move(i, distance * sin(currentAngle), distance * cos(currentAngle));
    }
    m_previoustime = currenttime;
}


/// 'imprints' pressure pulses in given array
void PulseControl::computePulsePattern(REAL currenttime, REAL dt,
                                       const Array2D<REAL>* pNetHeight,
                                       Array2D<REAL>** ppPressurePattern)
{
    m_pPulseManager->computePulsePattern(currenttime, dt, pNetHeight, ppPressurePattern);
}


const Geometry* PulseControl::getGeometry(int nr) const
{
    return m_pGeometryManager->getGeometry(nr);
}

